//
// Created by ysbf on 9/8/20.
//

#ifndef MYECL_LIB_DRIVERS_STM32_SENSORS_L3GD20_L3GD20_H
#define MYECL_LIB_DRIVERS_STM32_SENSORS_L3GD20_L3GD20_H
#include <utility>
#include <cstring>

#include "stdint.h"
#include "l3gd20_reg.h"

class L3GD20 {
public:
  typedef void (*RWRegCallback)(uint16_t);
  typedef void(*ReadCallback)();
  typedef void (*spi_read_type)(const uint8_t *buffer, uint16_t len);
  typedef void (*spi_write_type)(const uint8_t *buffer, uint16_t len);
  typedef void (*spi_async_read_type)(const uint8_t *buffer, uint16_t len, void callback_fun(uint16_t));
  typedef void (*spi_async_write_type)(const uint8_t *buffer, uint16_t len, void callback_fun(uint16_t));
  typedef void (*delay_type)(uint16_t);

  L3GD20(spi_read_type spi_read, spi_write_type spi_write, spi_async_read_type spi_async_read,
  spi_async_write_type spi_async_write, delay_type delay): m_spi_read(spi_read), m_spi_write(spi_write), m_spi_async_read(spi_async_read), m_spi_async_write(spi_async_write), m_delay(delay) {

  };
  typedef enum {
    GYRO_RANGE_250DPS = 250,
    GYRO_RANGE_500DPS = 500,
    GYRO_RANGE_2000DPS = 2000
  } gyroRange_t;

  void get_gyro_data(float *gyro);
  uint8_t check() {
    uint8_t ok = read_reg(WHO_AM_I);

    if (ok == 0xd4) {
      return 1;
    }
    return 0;
  }

  bool init(gyroRange_t range);




  void async_read_reg(uint8_t reg, uint8_t &value, const RWRegCallback callback) {
    static RWRegCallback callback_fun;
    callback_fun = callback;
    static uint8_t *pvalue;
    pvalue = &value;

    static uint8_t *pspi_buffer;
    pspi_buffer = spi_buffer;

    spi_buffer[0] = reg | 0x80u;
    spi_buffer[1] = 0xff;
    m_spi_async_read(spi_buffer, 2, [](uint16_t bytes_transferred) {
      *pvalue = pspi_buffer[1];
      callback_fun(bytes_transferred);
    });
  };


  void async_read_regs(uint8_t reg, uint8_t *buffer, uint8_t len, RWRegCallback const callback) {
    static RWRegCallback callback_fun;
    callback_fun = callback;
    static uint8_t *pbuffer;
    pbuffer = buffer;
    static uint8_t *pspi_buffer;
    pspi_buffer = spi_buffer;
    static uint8_t length;
    length = len;

    spi_buffer[0] = reg | 0x80u;
    //  if (len > 32) {
    //    return ;
    //  }
    //  static auto callback = callback_fun;
    memset(spi_buffer + 1, 0xff, len);
    m_spi_async_read(spi_buffer, len + 1, [](uint16_t bytes_transferred) {
      memcpy(pbuffer, pspi_buffer + 1, length);
      //    my_printf("read_regs\r\n");
      callback_fun(bytes_transferred);
    });
  };

  void async_write_reg(uint8_t reg, uint8_t value, RWRegCallback const callback) {
    static RWRegCallback callback_fun;
    callback_fun = callback;
    static uint8_t *pvalue;
    pvalue = &value;
    static uint8_t *pspi_buffer;
    pspi_buffer = spi_buffer;

    spi_buffer[0] = reg & 0x7fu;
    spi_buffer[1] = 0xff;
    m_spi_async_read(spi_buffer, 2, [](uint16_t bytes_transferred) {
      *pvalue = pspi_buffer[1];
      callback_fun(bytes_transferred);
    });
  }

  void async_write_regs(uint8_t reg, uint8_t *buffer, uint8_t len, RWRegCallback const callback) {

    static RWRegCallback callback_fun;
    callback_fun = callback;
    static uint8_t *pbuffer;
    pbuffer = buffer;
    static uint8_t *pspi_buffer;
    pspi_buffer = spi_buffer;
    static uint8_t length;
    length = len;

    spi_buffer[0] = (reg & 0x7fu);
    if (len > 32) {
      return;
    }
    memset(spi_buffer + 1, 0xff, len);
    m_spi_async_read(spi_buffer, len + 1, [](uint16_t bytes_transferred) {
      memcpy(pbuffer, pspi_buffer + 1, length);
      //    my_printf("read_regs\r\n");
      callback_fun(bytes_transferred);
    });
  }

  uint8_t read_reg(uint8_t reg) {
    reg|=0x80;
    uint8_t temp[2]{reg ,0};
    m_spi_write(temp, 2);
    return temp[1];
  };

  void write_reg(uint8_t reg, uint8_t value) {
    reg &= (uint8_t) 0x7fu;
    uint8_t temp[2]{reg, value};
    m_spi_write(temp, 2);
  };


  void read_regs(uint8_t reg, const uint8_t *buffer, uint8_t len){
    uint8_t temp[len + 1];
    memset(temp,0,len+1);
    temp[0] = reg | 0xc0u;
    m_spi_write(temp, len + 1);
    memcpy((void *) buffer, temp + 1, len);

    //    SPI_Device::read_regs(reg,buffer,len);
  };


private:
  gyroRange_t m_range;
  spi_read_type m_spi_read;
  spi_write_type m_spi_write;

  spi_async_read_type m_spi_async_read;
  spi_async_write_type m_spi_async_write;

  delay_type m_delay;
  uint8_t spi_buffer[33]{0};
};

#endif //MYECL_LIB_DRIVERS_STM32_SENSORS_L3GD20_L3GD20_H
